#include "controllor.h"
#include <thread>         
#include <chrono> 
#include "json.hpp"
using json = nlohmann::json;


const int timeout = 10;

void controllor::update_json_param(void)
{
    std::ifstream file("../parameters.json");
    json j;
    file >> j; 
    forward_ctl_.set_pid(j["pid"]["forward"]["p"],\
                        j["pid"]["forward"]["i"],\
                        j["pid"]["forward"]["d"]); 
    yaw_ctl_.set_pid(j["pid"]["yaw"]["p"],\
                    j["pid"]["yaw"]["i"],\
                    j["pid"]["yaw"]["d"]); 
    forward_offset_ = j["forward_offset"];
    yaw_offset_     = j["yaw_offset"];
    runtime_update_ = j["pid"]["runtime_update"];
}

controllor::controllor(const std::string& port_name )
{
    serial_ = std::make_shared<serial_ctl>(port_name);
    // forward_ctl_.set_pid(30.0f, .0f, .0f);
    // yaw_ctl_.set_pid(1500.0f, .0f, .0f);
    update_json_param();
}
    
controllor::~controllor()
{
    stop();
}

void controllor::set_connector(const std::shared_ptr<connector<float, float, float, int>> &con)
{
    pos_con_ = con;
}

void controllor::start(void)
{
    auto runable = [this] {
        // float 
        float x,y,z;
        float out_forward = 0, out_yaw = 0;
        int index, prev_index, no_signal = 0, cnt = 0;
        while(!is_stop_.load())
        {
            this->pos_con_->read(x, y, z, index);
            if(index == prev_index)
                no_signal++;
            else
                no_signal = 0;
            prev_index = index;
                
            if(no_signal > timeout)
                serial_->send_cmd(0, 0);
            else
            {
                float yaw = atan2(x, z);
                this->forward_ctl_.calculate(out_forward, z, forward_offset_);
                this->yaw_ctl_.calculate(out_yaw, yaw, yaw_offset_);  
                serial_->send_cmd(out_forward, -out_yaw);
                // serial_->send_cmd(100, 0);
            }
            std::cout << "controllor index"<< out_forward<<" "<< out_yaw << std::endl;
            // std::cout << forward_ctl_.param_info() << std::endl;
            std::this_thread::sleep_for(std::chrono::milliseconds(40));
            if((cnt++)%20 == 0 && runtime_update_)
                update_json_param();
        }
    };

    t_ = std::move(std::thread(runable));
    std::cout << "controllor service start"<< std::endl;
    
}

void controllor::stop(void)
{
    is_stop_.store(true);
    if(t_.joinable())
    {
        t_.join();
    }
    serial_ = nullptr;
    std::cout << "controllor service stop "<< std::endl;

}

